import com.ridgesoft.io.Speaker;
import com.ridgesoft.io.Display;
import com.ridgesoft.intellibrain.IntelliBrain;

import com.ridgesoft.robotics.RangeFinder;
import com.ridgesoft.robotics.SonarRangeFinder;

import com.ridgesoft.robotics.sensors.SharpGP2D12;
import com.ridgesoft.robotics.sensors.ParallaxPing;

import com.ridgesoft.robotics.Motor;
import com.ridgesoft.robotics.ContinuousRotationServo;

import com.ridgesoft.robotics.AnalogInput;

public class Driver {

	Display display;
	Motor lmoto, rmoto;
	AnalogInput ls;
	Speaker sound = IntelliBrain.getBuzzer();

	Driver(){
			
		display = IntelliBrain.getLcdDisplay();
		display.print(0, "SUMO!!!");
		
		ls =  IntelliBrain.getAnalogInput(6);
        lmoto = new ContinuousRotationServo(IntelliBrain.getServo(1), true, 14);
		rmoto = new ContinuousRotationServo(IntelliBrain.getServo(2), true, 14);
			
		//checkUS();
		
		moveUp();

		try{
			Thread.sleep(3000);
		}catch(Throwable t){
			System.out.println("thread failed!!");
		}
		
			//check US - checks ultrasound and calls function ^ if false.
			//if true backs the robot up, sleeps, moves it to start.
	}
	
	private void moveUp(){
		int theSample;
		lmoto.setPower(-8);
		rmoto.setPower(9);
		
		try{Thread.sleep(500);}catch(Throwable t){}
		
		while(true){
			theSample = ls.sample();
			display.print(1, ""+theSample+"");
			if(theSample < 100){
				display.print(1, "WHITE");
				lmoto.setPower(6);
				rmoto.setPower(-4);
				
				try{Thread.sleep(1000);}catch(Throwable t){}
				
				checkForCup();
			}
			
		}

	}

	private void checkForCup(){
		lmoto.setPower(6);
		rmoto.setPower(6);
			
		float distance;
		
		SonarRangeFinder sonar = new ParallaxPing(IntelliBrain.getDigitalIO(3));
		while(true){
			try{
					sonar.ping();
							try{Thread.sleep(300);}catch(Throwable t){t.printStackTrace();}
							
						distance = sonar.getDistanceInches();
						display.print(0, ""+distance+"");
						
						if(distance < 16 && distance > 0){
							lmoto.setPower(-8);
							rmoto.setPower(9);
							break;
						}
			}catch(Throwable t){System.out.println("error in US Method");}
		}
	}
    public static void main(String args[]) {
        new Driver();
    }

}